// 中文 utf8
#include <algorithm>
#include <cassert>
#include <float.h>
#include <vector>
#include <memory>
#include "aoi_impl.h"
#include <time.h>

typedef std::vector<engine_id_t> engine_id_arr;

AOI_EXPORT void * create_aoi_mgr(void * p_user_data, cb_a_enter_aoi_of_b a_enter_aoi_of_b, cb_a_leave_aoi_of_b a_leave_aoi_of_b, cb_a_batch_enter_leave_aoi a_batch_enter_leave_aoi, float aoi_deviation, bool enable_max_aoi_range_check)
{
	aoi_mgr_t* p_aoi_mgr = new aoi_mgr_t(p_user_data, a_enter_aoi_of_b, a_leave_aoi_of_b, a_batch_enter_leave_aoi, aoi_deviation, enable_max_aoi_range_check);
	return p_aoi_mgr;
}

AOI_EXPORT void free_aoi_mgr(void * p_aoi_mgr)
{
	delete (aoi_mgr_t*)p_aoi_mgr;
}

AOI_EXPORT void * aoi_mgr_get_user_data(void * p_aoi_mgr)
{
	return ((aoi_mgr_t*)p_aoi_mgr)->get_user_data();
}

AOI_EXPORT int aoi_set_aoi_deviation(void * p_aoi_mgr, float new_aoi_deviation)
{
	return ((aoi_mgr_t*)p_aoi_mgr)->set_aoi_deviation(new_aoi_deviation);
}

AOI_EXPORT int aoi_add_obj_to_scene(void * p_aoi_mgr, engine_id_t engine_id, float x, float y, float z, float aoi_range)
{
	return ((aoi_mgr_t*)p_aoi_mgr)->add(engine_id, x, y, z, aoi_range);
}

AOI_EXPORT int aoi_remove_obj_from_scene(void * p_aoi_mgr, engine_id_t engine_id)
{
	return ((aoi_mgr_t*)p_aoi_mgr)->remove(engine_id);
}

AOI_EXPORT int aoi_move_obj(void * p_aoi_mgr, engine_id_t engine_id, float new_x, float new_y, float new_z)
{
	return ((aoi_mgr_t*)p_aoi_mgr)->move_to(engine_id, new_x, new_y, new_z);
}

AOI_EXPORT int aoi_set_obj_aoi_range(void * p_aoi_mgr, engine_id_t engine_id, float new_aoi_range)
{
	return ((aoi_mgr_t*)p_aoi_mgr)->set_node_aoi_range(engine_id, new_aoi_range);
}

AOI_EXPORT int aoi_get_obj_info(void * p_aoi_mgr, engine_id_t engine_id, float * p_aoi_range, float * p_x, float * p_y, float * p_z, float * p_aoi_x, float * p_aoi_y, float * p_aoi_z)
{
	return ((aoi_mgr_t*)p_aoi_mgr)->get_node_info(engine_id, p_aoi_range, p_x, p_y, p_z, p_aoi_x, p_aoi_y, p_aoi_z);
}

AOI_EXPORT int aoi_iterate_this_see(void * p_aoi_mgr, engine_id_t engine_id, cb_aoi_iterate iterate_func)
{
	return ((aoi_mgr_t*)p_aoi_mgr)->iterate_this_see(engine_id, iterate_func);
}

AOI_EXPORT int aoi_iterate_see_this(void * p_aoi_mgr, engine_id_t engine_id, cb_aoi_iterate iterate_func)
{
	return ((aoi_mgr_t*)p_aoi_mgr)->iterate_see_this(engine_id, iterate_func);
}

AOI_EXPORT int aoi_get_see_rel(void * p_aoi_mgr, engine_id_t a, engine_id_t b)
{
	return ((aoi_mgr_t*)p_aoi_mgr)->get_see_rel(a, b);
}

AOI_EXPORT int aoi_set_relation(void * p_aoi_mgr, engine_id_t a, engine_id_t b, int rel)
{
	aoi_node_t* node_a = ((aoi_mgr_t*)p_aoi_mgr)->get_node_with_engine_id(a);
	aoi_node_t* node_b = ((aoi_mgr_t*)p_aoi_mgr)->get_node_with_engine_id(b);
	assert(NULL != node_a);
	assert(NULL != node_b);
	if (NULL == node_a || NULL == node_b) {
		return 0;
	}
	// 设定a能"看见"b， b也就一定能"看见"a
	// 当a检测到进入b的视野范围时，将a加入b的进入视野缓存中，所以这里需要把a从b的进入视野缓存中移除
	if (rel)
	{
		node_a->bitmap->set(b);
	}
	else
	{
		node_a->bitmap->clear(b);
	}
	return 0;
}

#define MIN_AOI_DEVIATION 0.1f

aoi_node_t::aoi_node_t(engine_id_t engine_id, float x, float y, float z, float aoi_range, int max_node_num) :
	p_x_prev(NULL),
	p_x_next(NULL),
	p_z_prev(NULL),
	p_z_next(NULL),
	x(x),
	y(y),
	z(z),
	aoi_x(x),
	aoi_y(y),
	aoi_z(z),
	aoi_range(aoi_range),
	engine_id(engine_id),
	bitmap(new Bitmap(max_node_num)),
	aoi_calc_filter(new int[max_node_num + 1])
{
	aoi_range_square = aoi_range * aoi_range;
	memset(aoi_calc_filter, -1, (max_node_num + 1)*sizeof(int));
}

aoi_node_t::~aoi_node_t()
{
	if (bitmap) {
		delete bitmap;
		bitmap = NULL;
	}
	if (aoi_calc_filter)
	{
		delete[] aoi_calc_filter;
		aoi_calc_filter = NULL;
	}
}

aoi_mgr_t::aoi_mgr_t(void * p_user_data, cb_a_enter_aoi_of_b a_enter_aoi_of_b, cb_a_leave_aoi_of_b a_leave_aoi_of_b, cb_a_batch_enter_leave_aoi a_batch_enter_leave_aoi, float aoi_deviation, bool enable_max_aoi_range_check) :
	p_user_data(p_user_data),
	p_x_head(NULL),
	p_z_head(NULL),
	a_enter_aoi_of_b(a_enter_aoi_of_b),
	a_leave_aoi_of_b(a_leave_aoi_of_b),
	a_batch_enter_leave_aoi(a_batch_enter_leave_aoi),
	max_node_num(0),
	aoi_deviation(aoi_deviation),
	max_aoi_range(FLT_MAX),
	debug_mode(0)
{
	assert(a_enter_aoi_of_b && a_leave_aoi_of_b && aoi_deviation >= 0.f);
	if (enable_max_aoi_range_check)
		max_aoi_range = 0.f;
	if (this->aoi_deviation <= MIN_AOI_DEVIATION)
		this->aoi_deviation = MIN_AOI_DEVIATION;
}

aoi_mgr_t::~aoi_mgr_t()
{
	for (aoi_nodes_table_t::iterator iter = aoi_nodes_table.begin(); iter != aoi_nodes_table.end(); iter++)
	{
		aoi_node_t* p_node = iter->second;
		free_aoi_node(p_node);
	}
	aoi_nodes_table.clear();

	p_x_head = NULL;
	p_z_head = NULL;
}

int aoi_mgr_t::set_aoi_deviation(float new_aoi_deviation)
{
	if (new_aoi_deviation < MIN_AOI_DEVIATION)
		new_aoi_deviation = MIN_AOI_DEVIATION;
	aoi_deviation = new_aoi_deviation;
	return 0;
}

void aoi_mgr_t::free_aoi_node(aoi_node_t * p_node)
{
	delete p_node;
}

inline int aoi_mgr_t::check_rel_on_move(const aoi_node_t * p_new, const aoi_node_t * p_other)
{
	int rel = 0;
	int new_rel = 0;
	// a,b 已可见，粗略检测是否离开视野
	if (p_new->bitmap->exists(p_other->engine_id))
	{
		new_rel = do_get_see_rel(p_new, p_other);
		if (!(new_rel & AOI_REL_ThisCanSeeOther))
		{
			rel |= AOI_REL_OtherLeaveThisAOI;
		}
	}
	else
	{
		new_rel = do_get_see_rel(p_new, p_other);
		if (new_rel & AOI_REL_ThisCanSeeOther)
		{
			rel |= AOI_REL_OtherEnterThisAOI;
		}
	}
	return rel;
}

aoi_node_t * aoi_mgr_t::alloc_aoi_node(engine_id_t engine_id, float x, float y, float z, float aoi_range)
{
	return new aoi_node_t(engine_id, x, y, z, aoi_range, max_node_num);
}

#define aoi_list_add_node(p_node, p_insert_after, axis) \
do { \
	if (p_insert_after) \
	{ \
		p_node->p_##axis##_next = p_insert_after->p_##axis##_next; \
		p_node->p_##axis##_prev = p_insert_after; \
		if (p_insert_after->p_##axis##_next) \
			p_insert_after->p_##axis##_next->p_##axis##_prev = p_node; \
		p_insert_after->p_##axis##_next = p_node; \
	} \
	else \
	{ \
		if (p_##axis##_head) \
		{ \
			p_node->p_##axis##_next = p_##axis##_head; \
			p_##axis##_head->p_##axis##_prev = p_node; \
			p_##axis##_head = p_node; \
		} \
		else \
		{ \
			p_##axis##_head = p_node; \
		} \
	} \
} while (0)

#define aoi_list_remove_node(p_node, axis) \
do { \
	if (p_node->p_##axis##_prev) \
		p_node->p_##axis##_prev->p_##axis##_next = p_node->p_##axis##_next; \
	if (p_node->p_##axis##_next) \
		p_node->p_##axis##_next->p_##axis##_prev = p_node->p_##axis##_prev; \
	if (p_##axis##_head == p_node) \
		p_##axis##_head = p_node->p_##axis##_next; \
	p_node->p_##axis##_prev = NULL; \
	p_node->p_##axis##_next = NULL; \
} while (0)

void aoi_mgr_t::set_max_node_num(int num)
{
	max_node_num = num;
}

int aoi_mgr_t::add(engine_id_t engine_id, float x, float y, float z, float aoi_range)
{
	if (aoi_nodes_table.find(engine_id) != aoi_nodes_table.end())
		return -1;

	assert(engine_id != INVALID_ENGINE_ID);
	assert(aoi_range >= 0.f && aoi_range < FLT_MAX);

	if (aoi_range < 0.f)
		aoi_range = 0.f;

	aoi_node_t * p_node = alloc_aoi_node(engine_id, x, y, z, aoi_range);
	if (!p_node)
		return -2;

	aoi_nodes_table.emplace(engine_id, p_node);

	if (max_aoi_range < aoi_range)
		max_aoi_range = aoi_range;

	aoi_node_t * p_x_insert_after = NULL;
	aoi_node_t * p_z_insert_after = NULL;

#define __add__enter_aoi_check(axis) \
do { \
	for (aoi_node_t * p_cur = p_##axis##_head; p_cur; p_cur = p_cur->p_##axis##_next) \
	{ \
		float axis_distance = p_node->aoi_##axis - p_cur->aoi_##axis; \
		if (axis_distance < 0) \
		{ \
			if (-axis_distance > max_aoi_range) \
				break; \
		} \
		else \
		{ \
			p_##axis##_insert_after = p_cur; \
		} \
		if (p_node->aoi_calc_filter[p_cur->engine_id] == -1) \
		{ \
			int rel = check_rel_on_add(p_node, p_cur); \
			if (rel) \
				p_node->aoi_calc_filter[p_cur->engine_id] = rel; \
		} \
	} \
} while (0)

	__add__enter_aoi_check(x);

	__add__enter_aoi_check(z);

	aoi_list_add_node(p_node, p_x_insert_after, x);
	aoi_list_add_node(p_node, p_z_insert_after, z);

	engine_id_arr enter_engine_ids;

	for (int i = 1; i <= max_node_num; i++)
	{
		int rel = p_node->aoi_calc_filter[i];
		if (rel <= 0) continue;
		if (rel & AOI_REL_OtherEnterThisAOI)
			enter_engine_ids.push_back(i);
	}

	if (!enter_engine_ids.empty()) {
		a_batch_enter_leave_aoi(this, engine_id, enter_engine_ids.size(), &enter_engine_ids.front(), 0, NULL);
	}

	validate();

	return 0;
}

int aoi_mgr_t::remove(engine_id_t engine_id)
{
	if (aoi_nodes_table.find(engine_id) == aoi_nodes_table.end())
		return -1;
	aoi_node_t * p_node = aoi_nodes_table.at(engine_id);

	aoi_nodes_table.erase(engine_id);

#define __remove__leave_aoi_check(p_axis, aoi_axis) \
do { \
	for (aoi_node_t * p_cur = p_node->p_axis; p_cur; p_cur = p_cur->p_axis) \
	{ \
		float axis_distance = std::abs(p_node->aoi_axis - p_cur->aoi_axis); \
		if (axis_distance > max_aoi_range) \
			break; \
		if (p_node->aoi_calc_filter[p_cur->engine_id] == -1) \
		{ \
			int rel = check_rel_on_del(p_node, p_cur); \
			if (rel) \
				p_node->aoi_calc_filter[p_cur->engine_id] = rel; \
		} \
	} \
} while (0)

	__remove__leave_aoi_check(p_x_prev, aoi_x);
	__remove__leave_aoi_check(p_x_next, aoi_x);

	__remove__leave_aoi_check(p_z_prev, aoi_z);
	__remove__leave_aoi_check(p_z_next, aoi_z);

	aoi_list_remove_node(p_node, x);
	aoi_list_remove_node(p_node, z);

	for (int i = 1; i <= max_node_num; i++)
	{
		int rel = p_node->aoi_calc_filter[i];
		if (rel <= 0) continue;
		if (rel & AOI_REL_ThisLeaveOtherAOI)
			a_leave_aoi_of_b(this, engine_id, i);
	}

	if (max_aoi_range > 0.f && max_aoi_range == p_node->aoi_range)
		update_max_aoi_range();

	free_aoi_node(p_node);

	validate();

	return 0;
}

void aoi_mgr_t::update_max_aoi_range()
{
	if (max_aoi_range == FLT_MAX)
		return;

	float old_max = max_aoi_range;
	float cur_max = 0.f;
	for (aoi_nodes_table_t::iterator iter = aoi_nodes_table.begin(); iter != aoi_nodes_table.end(); iter++)
	{
		float aoi_range = iter->second->aoi_range;
		if (old_max > 0.f)
		{
			assert(old_max >= aoi_range);
			if (old_max == aoi_range)
				return;
		}
		if (aoi_range > cur_max)
			cur_max = aoi_range;
	}
	max_aoi_range = cur_max;
}

int aoi_mgr_t::move_to(engine_id_t engine_id, float new_x, float new_y, float new_z)
{
	if (aoi_nodes_table.find(engine_id) == aoi_nodes_table.end())
		return -1;

	aoi_node_t * p_node = aoi_nodes_table.at(engine_id);
	if (p_node->x == new_x && p_node->y == new_y && p_node->z == new_z)
		return 0;

	aoi_node_t old;
	old.x = p_node->x;
	old.y = p_node->y;
	old.z = p_node->z;
	old.aoi_x = p_node->aoi_x;
	old.aoi_y = p_node->aoi_y;
	old.aoi_z = p_node->aoi_z;

	p_node->x = new_x;
	p_node->y = new_y;
	p_node->z = new_z;

	// aoi中轴上移动超过一定距离才算移动了
	if (std::abs(new_x - p_node->aoi_x) > aoi_deviation)
		p_node->aoi_x = new_x;
	if (std::abs(new_y - p_node->aoi_y) > aoi_deviation)
		p_node->aoi_y = new_y;
	if (std::abs(new_z - p_node->aoi_z) > aoi_deviation)
		p_node->aoi_z = new_z;

	if (old.aoi_x == p_node->aoi_x && old.aoi_z == p_node->aoi_z)
		return 0;

	old.aoi_range = p_node->aoi_range;
	old.aoi_range_square = p_node->aoi_range_square;

	aoi_node_t * p_x_insert_after = NULL;
	aoi_node_t * p_z_insert_after = NULL;

#define __move_to__aoi_check() \
do { \
	if (p_node->aoi_calc_filter[p_cur->engine_id] == -1) \
	{ \
		int rel = check_rel_on_move(p_node, p_cur); \
		{ \
			p_node->aoi_calc_filter[p_cur->engine_id] = rel; \
			p_cur->aoi_calc_filter[p_node->engine_id] = rel; \
		} \
	} \
} while (0)

	clock_t t1 = clock();
	// x 轴 prev
	for (aoi_node_t * p_cur = p_node->p_x_prev; p_cur; p_cur = p_cur->p_x_prev)
	{
		if (!p_x_insert_after && p_cur->aoi_x <= p_node->aoi_x)
			p_x_insert_after = p_cur;

		if (std::min(old.aoi_x, p_node->aoi_x) - p_cur->aoi_x > max_aoi_range)
			break;
		__move_to__aoi_check();
	}
	clock_t t2 = clock();

	// x 轴 next
	for (aoi_node_t * p_cur = p_node->p_x_next; p_cur; p_cur = p_cur->p_x_next)
	{
		if (p_cur->aoi_x < p_node->aoi_x) // 这里不能是 <=
			p_x_insert_after = p_cur;

		if (p_cur->aoi_x - std::max(old.aoi_x, p_node->aoi_x) > max_aoi_range)
			break;
		__move_to__aoi_check();
	}
	clock_t t3 = clock();

	// z 轴 prev
	for (aoi_node_t * p_cur = p_node->p_z_prev; p_cur; p_cur = p_cur->p_z_prev)
	{
		if (!p_z_insert_after && p_cur->aoi_z <= p_node->aoi_z)
			p_z_insert_after = p_cur;

		if (std::min(old.aoi_z, p_node->aoi_z) - p_cur->aoi_z > max_aoi_range)
			break;
		__move_to__aoi_check();
	}
	clock_t t4 = clock();

	// z 轴 next
	for (aoi_node_t * p_cur = p_node->p_z_next; p_cur; p_cur = p_cur->p_z_next)
	{
		if (p_cur->aoi_z < p_node->aoi_z) // 这里不能是 <=
			p_z_insert_after = p_cur;

		if (p_cur->aoi_z - std::max(old.aoi_z, p_node->aoi_z) > max_aoi_range)
			break;
		__move_to__aoi_check();
	}

	if (p_x_insert_after != p_node->p_x_prev)
	{
		aoi_list_remove_node(p_node, x);
		aoi_list_add_node(p_node, p_x_insert_after, x);
	}
	if (p_z_insert_after != p_node->p_z_prev)
	{
		aoi_list_remove_node(p_node, z);
		aoi_list_add_node(p_node, p_z_insert_after, z);
	}

	engine_id_arr enter_engine_ids, leave_engine_ids;

	for (int i = 1; i <= max_node_num; i++)
	{
		int rel = p_node->aoi_calc_filter[i];
		if (rel <= 0) continue;
		if (rel & AOI_REL_OtherEnterThisAOI)
			enter_engine_ids.push_back(i);
		if (rel & AOI_REL_OtherLeaveThisAOI)
			leave_engine_ids.push_back(i);
	}

	engine_id_t * enter_engine_ptr = enter_engine_ids.empty() ? NULL : &enter_engine_ids.front(),
		*leave_engine_ptr = leave_engine_ids.empty() ? NULL : &leave_engine_ids.front();
	if (enter_engine_ptr || leave_engine_ptr) {
		a_batch_enter_leave_aoi(this, engine_id, enter_engine_ids.size(), enter_engine_ptr, leave_engine_ids.size(), leave_engine_ptr);
	}

	validate();
	return 0;
}

int aoi_mgr_t::set_node_aoi_range(engine_id_t engine_id, float new_aoi_range)
{
	if (aoi_nodes_table.find(engine_id) == aoi_nodes_table.end())
		return -1;
	aoi_node_t * p_node = aoi_nodes_table.at(engine_id);

	assert(new_aoi_range >= 0.f && new_aoi_range < FLT_MAX);

	if (new_aoi_range < 0.f)
		new_aoi_range = 0.f;

	float old_aoi_range = p_node->aoi_range;
	if (new_aoi_range == old_aoi_range)
		return 0;
	float old_aoi_range_square = p_node->aoi_range_square;
	float new_aoi_range_square = new_aoi_range * new_aoi_range;

#define __set_node_aoi_range__check(p_axis, aoi_axis, rel, small_range_square, big_range_square, big_range) \
do { \
	for (aoi_node_t * p_cur = p_node->p_axis; p_cur; p_cur = p_cur->p_axis) \
	{ \
		float axis_distance = std::abs(p_node->aoi_axis - p_cur->aoi_axis); \
		if (axis_distance > big_range) \
			break; \
		if (p_node->aoi_calc_filter[p_cur->engine_id] == -1) \
		{ \
			if (axis_distance_in_range(p_node, p_cur, big_range)) \
			{ \
				float distance_square = get_aoi_distance_square(p_node, p_cur); \
				if ((small_range_square == 0.f || distance_square > small_range_square) && distance_square <= big_range_square) \
					p_node->aoi_calc_filter[p_cur->engine_id] = rel; \
			} \
		} \
	} \
} while (0)

	if (new_aoi_range > old_aoi_range)
	{
		__set_node_aoi_range__check(p_x_prev, aoi_x, AOI_REL_OtherEnterThisAOI, old_aoi_range_square, new_aoi_range_square, new_aoi_range);
		__set_node_aoi_range__check(p_x_next, aoi_x, AOI_REL_OtherEnterThisAOI, old_aoi_range_square, new_aoi_range_square, new_aoi_range);
		__set_node_aoi_range__check(p_z_prev, aoi_z, AOI_REL_OtherEnterThisAOI, old_aoi_range_square, new_aoi_range_square, new_aoi_range);
		__set_node_aoi_range__check(p_z_next, aoi_z, AOI_REL_OtherEnterThisAOI, old_aoi_range_square, new_aoi_range_square, new_aoi_range);
	}
	else
	{
		__set_node_aoi_range__check(p_x_prev, aoi_x, AOI_REL_OtherLeaveThisAOI, new_aoi_range_square, old_aoi_range_square, old_aoi_range);
		__set_node_aoi_range__check(p_x_next, aoi_x, AOI_REL_OtherLeaveThisAOI, new_aoi_range_square, old_aoi_range_square, old_aoi_range);
		__set_node_aoi_range__check(p_z_prev, aoi_z, AOI_REL_OtherLeaveThisAOI, new_aoi_range_square, old_aoi_range_square, old_aoi_range);
		__set_node_aoi_range__check(p_z_next, aoi_z, AOI_REL_OtherLeaveThisAOI, new_aoi_range_square, old_aoi_range_square, old_aoi_range);
	}

	p_node->aoi_range = new_aoi_range;
	p_node->aoi_range_square = new_aoi_range_square;

	engine_id_arr enter_engine_ids, leave_engine_ids;

	for (int i = 1; i <= max_node_num; i++)
	{
		int rel = p_node->aoi_calc_filter[i];
		if (rel <= 0) continue;
		if (rel & AOI_REL_OtherEnterThisAOI)
			enter_engine_ids.push_back(i);
		if (rel & AOI_REL_OtherLeaveThisAOI)
			leave_engine_ids.push_back(i);
	}

	engine_id_t * enter_engine_ptr = enter_engine_ids.empty() ? NULL : &enter_engine_ids.front(),
		*leave_engine_ptr = leave_engine_ids.empty() ? NULL : &leave_engine_ids.front();
	if (enter_engine_ptr || leave_engine_ptr) {
		a_batch_enter_leave_aoi(this, engine_id, enter_engine_ids.size(), enter_engine_ptr, leave_engine_ids.size(), leave_engine_ptr);
	}

	if (new_aoi_range > old_aoi_range)
	{
		if (new_aoi_range > max_aoi_range)
			max_aoi_range = new_aoi_range;
	}
	else
	{
		if (old_aoi_range == max_aoi_range)
			update_max_aoi_range();
	}

	validate();

	return 0;
}

int aoi_mgr_t::get_node_info(engine_id_t engine_id, float * p_aoi_range, float * p_x, float * p_y, float * p_z, float * p_aoi_x, float * p_aoi_y, float * p_aoi_z)
{
	if (aoi_nodes_table.find(engine_id) == aoi_nodes_table.end())
		return -1;
	aoi_node_t * p_node = aoi_nodes_table.at(engine_id);

	if (p_aoi_range)
		*p_aoi_range = p_node->aoi_range;

	if (p_x)
		*p_x = p_node->x;
	if (p_y)
		*p_y = p_node->y;
	if (p_z)
		*p_z = p_node->z;

	if (p_aoi_x)
		*p_aoi_x = p_node->aoi_x;
	if (p_aoi_y)
		*p_aoi_y = p_node->aoi_y;
	if (p_aoi_z)
		*p_aoi_z = p_node->aoi_z;

	return 0;
}

aoi_node_t* aoi_mgr_t::get_node_with_engine_id(engine_id_t engine_id)
{
	aoi_nodes_table_t::iterator itor = aoi_nodes_table.find(engine_id);
	return itor == aoi_nodes_table.end() ? NULL : itor->second;
}

int aoi_mgr_t::iterate_this_see(engine_id_t engine_id, cb_aoi_iterate iterate_func)
{
	if (aoi_nodes_table.find(engine_id) == aoi_nodes_table.end())
		return -1;
	aoi_node_t * p_node = aoi_nodes_table.at(engine_id);

#define __iterate_this_see__do(p_axis, aoi_axis) \
do { \
	for (aoi_node_t * p_cur = p_node->p_axis; p_cur; p_cur = p_cur->p_axis) \
	{ \
		if (std::abs(p_node->aoi_axis - p_cur->aoi_axis) > p_node->aoi_range) \
			break; \
		if (p_node->aoi_calc_filter[p_cur->engine_id] == -1) \
		{ \
			if (axis_distance_in_range(p_node, p_cur, p_node->aoi_range) && get_aoi_distance_square(p_node, p_cur) <= p_node->aoi_range_square) \
			{ \
				p_node->aoi_calc_filter[p_cur->engine_id] = AOI_REL_ThisCanSeeOther; \
				iterate_func(this, engine_id, p_cur->engine_id); \
			} \
		} \
	} \
} while (0)

	// x 轴 
	__iterate_this_see__do(p_x_prev, aoi_x);
	__iterate_this_see__do(p_x_next, aoi_x);

	// z 轴 
	__iterate_this_see__do(p_z_prev, aoi_z);
	__iterate_this_see__do(p_z_next, aoi_z);

	return 0;
}

int aoi_mgr_t::iterate_see_this(engine_id_t engine_id, cb_aoi_iterate iterate_func)
{
	if (aoi_nodes_table.find(engine_id) == aoi_nodes_table.end())
		return -1;
	aoi_node_t * p_node = aoi_nodes_table.at(engine_id);

#define __iterate_see_this__do(p_axis, aoi_axis) \
do { \
	for (aoi_node_t * p_cur = p_node->p_axis; p_cur; p_cur = p_cur->p_axis) \
	{ \
		float distance = std::abs(p_cur->aoi_axis - p_node->aoi_axis); \
		if (distance > max_aoi_range) \
			break; \
		if (p_node->aoi_calc_filter[p_cur->engine_id] == -1) \
		{ \
			if (axis_distance_in_range(p_node, p_cur, p_cur->aoi_range) && get_aoi_distance_square(p_node, p_cur) <= p_cur->aoi_range_square) \
			{ \
				p_node->aoi_calc_filter[p_cur->engine_id] = AOI_REL_OtherCanSeeThis; \
				iterate_func(this, p_cur->engine_id, engine_id); \
			} \
		} \
	} \
} while (0)

	// x 轴 
	__iterate_see_this__do(p_x_prev, aoi_x);
	__iterate_see_this__do(p_x_next, aoi_x);

	// z 轴 
	__iterate_see_this__do(p_z_prev, aoi_z);
	__iterate_see_this__do(p_z_next, aoi_z);

	return 0;
}

int aoi_mgr_t::get_see_rel(engine_id_t a, engine_id_t b)
{
	if (aoi_nodes_table.find(a) == aoi_nodes_table.end() || aoi_nodes_table.find(b) == aoi_nodes_table.end())
		return 0;

	aoi_node_t * p_a = aoi_nodes_table.at(a);
	aoi_node_t * p_b = aoi_nodes_table.at(b);

	return do_get_see_rel(p_a, p_b);
}

void aoi_mgr_t::validate()
{
	if (debug_mode == 0)
		return;

	aoi_node_t * p_prev;

	p_prev = NULL;
	for (aoi_node_t * p_cur = p_x_head; p_cur; p_cur = p_cur->p_x_next)
	{
		assert(p_cur->aoi_range <= max_aoi_range);
		assert(std::abs(p_cur->aoi_x - p_cur->x) <= aoi_deviation);
		if (p_prev)
		{
			assert(p_prev->aoi_x <= p_cur->aoi_x);
		}
		p_prev = p_cur;
	}

	p_prev = NULL;
	for (aoi_node_t * p_cur = p_z_head; p_cur; p_cur = p_cur->p_z_next)
	{
		assert(std::abs(p_cur->aoi_z - p_cur->z) <= aoi_deviation);
		if (p_prev)
		{
			assert(p_prev->aoi_z <= p_cur->aoi_z);
		}
		p_prev = p_cur;
	}
}
